
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       derivative_filter6.c
  * @author     baiyang
  * @date       2021-11-7
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "derivative_filter.h"

#include <math.h>
#include <float.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/**
  * @brief       
  * @param[in]   filter  
  * @param[out]  
  * @retval      
  * @note        
  */
void derivative_filter6(DerivativeFilterFloat_Size6 *filter)
{
    filter->sample_index = 0;

    derivative_filter6_reset(filter);
}

// reset - clear the filter
void derivative_filter6_reset(DerivativeFilterFloat_Size6 *filter)
{
    // clear samples buffer
    for( int8_t i=0; i<DERIVTIVE_FILTER_SIZE_6; i++ ) {
        filter->samples[i] = 0;
    }

    // reset index back to beginning of the array
    filter->sample_index = 0;
}

void derivative_filter6_update(DerivativeFilterFloat_Size6 *filter, float sample, uint32_t timestamp)
{
    uint8_t i = filter->sample_index;
    uint8_t i1;
    if (i == 0) {
        i1 = DERIVTIVE_FILTER_SIZE_6-1;
    } else {
        i1 = i-1;
    }
    if (filter->_timestamps[i1] == timestamp) {
        // this is not a new timestamp - ignore
        return;
    }

    // add timestamp before we apply to FilterWithBuffer
    filter->_timestamps[i] = timestamp;

    // call parent's apply function to get the sample into the array
    derivative_filter6_apply(filter, sample);

    filter->_new_data = true;
}

float derivative_filter6_apply(DerivativeFilterFloat_Size6 *filter, float sample)
{
    // add sample to array
    filter->samples[filter->sample_index++] = sample;

    // wrap index if necessary
    if (filter->sample_index >= DERIVTIVE_FILTER_SIZE_6)
        filter->sample_index = 0;

    // base class doesn't know what filtering to do so we just return the raw sample
    return sample;
}

float derivative_filter6_slope(DerivativeFilterFloat_Size6 *filter)
{
    if (!filter->_new_data) {
        return filter->_last_slope;
    }

    float result = 0;

    // use f() to make the code match the maths a bit better. Note
    // that unlike an average filter, we care about the order of the elements
#define f(i) filter->samples[(((filter->sample_index-1)+i+1)+3*DERIVTIVE_FILTER_SIZE_6/2) % DERIVTIVE_FILTER_SIZE_6]
#define x(i) filter->_timestamps[(((filter->sample_index-1)+i+1)+3*DERIVTIVE_FILTER_SIZE_6/2) % DERIVTIVE_FILTER_SIZE_6]

    if (filter->_timestamps[DERIVTIVE_FILTER_SIZE_6-1] == filter->_timestamps[DERIVTIVE_FILTER_SIZE_6-2]) {
        // we haven't filled the buffer yet - assume zero derivative
        return 0;
    }

    // N in the paper is FILTER_SIZE
    switch (DERIVTIVE_FILTER_SIZE_6) {
    case 5:
        result = 2*2*(f(1) - f(-1)) / (x(1) - x(-1))
                 + 4*1*(f(2) - f(-2)) / (x(2) - x(-2));
        result /= 8;
        break;
    case 7:
        result = 2*5*(f(1) - f(-1)) / (x(1) - x(-1))
                 + 4*4*(f(2) - f(-2)) / (x(2) - x(-2))
                 + 6*1*(f(3) - f(-3)) / (x(3) - x(-3));
        result /= 32;
        break;
    case 9:
        result = 2*14*(f(1) - f(-1)) / (x(1) - x(-1))
                 + 4*14*(f(2) - f(-2)) / (x(2) - x(-2))
                 + 6* 6*(f(3) - f(-3)) / (x(3) - x(-3))
                 + 8* 1*(f(4) - f(-4)) / (x(4) - x(-4));
        result /= 128;
        break;
    case 11:
        result =  2*42*(f(1) - f(-1)) / (x(1) - x(-1))
                 +  4*48*(f(2) - f(-2)) / (x(2) - x(-2))
                 +  6*27*(f(3) - f(-3)) / (x(3) - x(-3))
                 +  8* 8*(f(4) - f(-4)) / (x(4) - x(-4))
                 + 10* 1*(f(5) - f(-5)) / (x(5) - x(-5));
        result /= 512;
        break;
    default:
        result = 0;
        break;
    }

    // cope with numerical errors
    if (isnan(result) || isinf(result)) {
        result = 0;
    }

    filter->_new_data = false;
    filter->_last_slope = result;

    return result;
}

/*------------------------------------test------------------------------------*/


